#!/usr/bin/env python
 
import rospy
import time
import actionlib
from agv_actions.msg import PeripheralAction, PeripheralGoal, PeripheralFeedback, PeripheralResult
from agv_msgs.msg import AvoidObsta, PairTypeInt, ReadInPutsNew, AvoidObstaInfo, PeriphStatus, AGVEvent, V_action, V_actionParam
from agv_msgs.msg import AgvAutoStatus, OdometryLite
from geometry_msgs.msg import Pose, Twist
from tf.transformations import euler_from_quaternion
from std_msgs.msg import Time
from std_msgs.msg import Header

header_seq = 0 
agv_load_status = 0
rack_follow_up = 0

def create_header():
    global header_seq
    header = Header()
    header.seq = header_seq
    header.stamp = rospy.Time.now()
    header.frame_id = "peripheral"
    header_seq = header_seq + 1
    return header
 
class PeripheralActionServer:
    def __init__(self):
        self._action_name = '/do_peripheral'
        self._server = actionlib.SimpleActionServer(self._action_name, PeripheralAction, self.execute_cb, False)
        self._server.start()
        print('start Peripheral action server.')

    def execute_cb(self, goal):
         # 初始化结果和反馈
        result = PeripheralResult()
        feedback = PeripheralFeedback()
 
        # 检查目标动作是否有效 # 0:None  1:up  2:down  3:clockwise rotary  4:anticlockwise rotary
        if goal.goal_action.actionType not in ["0", "1", "2", "3", "4"]:
            rospy.logwarn('%s: Invalid goal action received: %s', self._action_name, goal.goal_action.actionType)
            result.success = 1  # Failed
            result.message = "Invalid goal action"
            self._server.set_aborted(result, "Aborted due to invalid goal action")
            return
 
        # 模拟执行动作
        rospy.loginfo('%s: Executing goal action: %s', self._action_name, goal.goal_action)
        feedback.state = 1  # Running
        for i in range(1, 11):  # 模拟10个进度点
            time.sleep(0.1)     # 假设每个动作需要100毫秒
            feedback.percent_complete = i*10
            feedback.feedback_action = V_action()  # 这里需要填充V_action的具体字段，或者保持为空
            feedback.feedback_action = goal.goal_action
            self._server.publish_feedback(feedback)
 
        # 根据模拟情况设置结果
        if goal.goal_action.actionType == "0":  # None action, always succeed
            result.success = 0
            result.message = "Normal success (No action taken)"
        else:
            global agv_load_status
            # 对于其他动作，我们假设它们都成功执行
            result.success = 0
            result.message = "Action executed successfully"
            result.result_action = V_action()  # 同样需要填充V_action的具体字段，或者根据逻辑设置
            result.result_action = goal.goal_action
            if goal.goal_action.actionType == "1":
                agv_load_status = 1
            elif goal.goal_action.actionType == "2":
                agv_load_status = 0
       
        self._server.set_succeeded(result, "Goal succeeded")

class PeripheralSubscriber:
    def __init__(self):
        # 订阅 AgvAutoStatus 话题
        self.auto_run_status_sub = node.create_subscription(AgvAutoStatus, "auto_run_status", self.status_callback)
        self.odom_sub = node.create_subscription(OdometryLite, "/odom", self.odom_callback)
        print('start Peripheral subscriber.')
    def status_callback(self, data):
        global rack_follow_up
        # 处理接收到的消息
        # rospy.loginfo("Received AgvAutoStatus message:")
        # self.log_field("fProgress", data.fProgress)
        # self.log_field("fProgPercent", data.fProgPercent)
        # self.log_field("curPst_x", data.curPst_x)
        # self.log_field("curPst_y", data.curPst_y)
        # self.log_field("curPst_t", data.curPst_t)
        # self.log_field("curVel_x", data.curVel_x)
        # self.log_field("curVel_z", data.curVel_z)
        # self.log_field("velLevel", data.velLevel)
        # self.log_field("fDevProg", data.fDevProg)
        # self.log_field("fDev", data.fDev)
        # self.log_field("fDevTheta", data.fDevThita)
        # self.log_field("fQuality", data.fQuality)
        # self.log_field("event", data.event)
        # self.log_field("workState", data.workState)
        # self.log_field("wheelMoveDir", data.wheelMoveDir)
        # self.log_field("trajType", data.trajType)
        # self.log_field("locateType", data.locateType)
        # self.log_field("scenes", data.scenes)
        # self.log_field("bits", data.bits)
        # self.log_field("word1", data.word1)
        # self.log_field("word2", data.word2)
        # self.log_field("pathWord1", data.pathWord1)
        # self.log_field("pathWord2", data.pathWord2)
        if rack_follow_up != data.rackFollowUp:
            self.log_field("rackFollowUp", data.rackFollowUp)
            rack_follow_up = data.rackFollowUp
        # self.log_field("navigationType", data.navigationType)
        # self.log_field("driving", data.driving)
        # self.log_field("startDelay", data.startDelay)
        # self.log_field("paused", data.paused)
        # self.log_field("idle", data.idle)
        # self.log_field("orderIdNew", data.orderIdNew)
        # self.log_field("orderUpdateIdNew", data.orderUpdateIdNew)
        # self.log_field("nodeIdNew", data.nodeIdNew)
   
    def odom_callback(self, data):
        qx = data.pose.orientation.x
        qy = data.pose.orientation.y
        qz = data.pose.orientation.z
        qw = data.pose.orientation.w
        roll, pitch, yaw = roll, pitch, yaw = euler_from_quaternion([qx, qy, qz, qw])
        
    def log_field(self, name, value):
        # 辅助函数，用于打印字段名和值
        rospy.loginfo(f"{name}: {value}")

class PeripheralPublisher:
    def __init__(self):
        self.read_input_pub = node.create_publisher(ReadInPutsNew, queue_size=10, '/readInputNew')
        self.avoid_obsta_pub = node.create_publisher(AvoidObstaInfo, queue_size=10, '/agv_obstacle_level')
        self.periph_status_pub = node.create_publisher(PeriphStatus, queue_size=10, '/periphStatus')
        self.agv_event_pub = node.create_publisher(AGVEvent, queue_size=10, '/agvevent')
        self.rate = rospy.Rate(15)  # 定义发布频率
        print('start Peripheral publisher.')
        self.run()
        
    def run(self):
        while not rospy.is_shutdown():
            self.publish_read_input()
            self.publish_avoid_obsta()
            self.publish_periph_status()
            self.publish_agv_event()
            self.rate.sleep()
 
    def publish_read_input(self):
        read_input_msg = ReadInPutsNew()
        estop = PairTypeInt()
        bumper = PairTypeInt()
        reset = PairTypeInt()
        stop = PairTypeInt()
        release = PairTypeInt()
        estop.key = "emergencyButton"
        bumper.key = "bumper"
        reset.key = "resetButton"
        stop.key = "stopButton"
        release.key = "releaseButton"
        read_input_msg.array.append(estop)
        read_input_msg.array.append(bumper)
        read_input_msg.array.append(reset)
        read_input_msg.array.append(stop)
        read_input_msg.array.append(release)
        self.read_input_pub.publish(read_input_msg)
 
    def publish_avoid_obsta(self):
        avoid_obsta_msg = AvoidObstaInfo()
        time_now = rospy.Time.now()
        avoid_obsta_msg.time.data.secs = int(time_now.to_sec())
        avoid_obsta_msg.scenes_name = "fake_obsta_msg"
        self.avoid_obsta_pub.publish(avoid_obsta_msg)
 
    def publish_periph_status(self):
        global agv_load_status
        periph_status_msg = PeriphStatus()
        periph_status_msg.header = create_header()
        periph_status_msg.periphStateInfo = 1
        periph_status_msg.loaded = agv_load_status
        self.periph_status_pub.publish(periph_status_msg)
 
    def publish_agv_event(self):
        event_msg = AGVEvent()
        self.agv_event_pub.publish(event_msg)
 
if __name__ == '__main__':
    try:
        rclpy.init()
        node = rclpy.create_node('fake_peripheral', anonymous=True)
        server = PeripheralActionServer()
        subscriber = PeripheralSubscriber()
        publisher = PeripheralPublisher()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass